#include <stdlib.h>
#include "../cominc/io_sbus.h"

/*SBUS Module*/
static SBusObj *RX = NULL;

sbusError SBusInit(/*const uint8_t *rom, int romLen,*/ const uint8_t *rxBuffer) // 初始化sbus单例
{
	RX = (SBusObj *)malloc(sizeof(SBusObj));
	// memcpy((uint8_t *)RX, rom, romLen);
	RX->raw = rxBuffer;
	// rst all channel
	for (int ch = 0; ch < SBUS_CH_MAX; ch++)
	{
		RX->ch[ch] = SBUS_CH_VAL_DEFAULT;
	}

	// stub for config file
	for (int ch = 0; ch < SBUS_CH_MAX; ch++)
	{
		RX->cfg[ch].failSafeVal = SBUS_CH_VAL_DEFAULT;
	}
	RX->cfg[0].failSafeVal = 960;
	RX->cfg[1].failSafeVal = 980;
	RX->cfg[2].failSafeVal = 310;
	RX->cfg[3].failSafeVal = 980;
	RX->comFailSta = SBUS_COM_FAIL_START;
	return NO_ERR;
}

sbusError SBusDecode(int rxLen)
{
	if ((RX == NULL) || (RX->raw[0] != 0x0F) || (RX->raw[24] != 0x00) || rxLen != SBUS_RAW_LEN)
	{
		return FRM_ERROR;
	}
	if (RX->raw[23])
	{
		// remoter offline
		for (int ch = 0; ch < SBUS_CH_MAX; ch++)
		{
			RX->ch[ch] = RX->cfg[ch].failSafeVal;
		}
		RX->comFailSta = SBUS_COM_FAIL_START;
		return RX_LOST;
	}
	RX->comFailSta = SBUS_COM_FAIL_END;
	// frame is aligned
	RX->ch[0] = (int16_t)(RX->raw[2] & 0x07) << 8 | RX->raw[1];
	RX->ch[1] = (int16_t)(RX->raw[3] & 0x3f) << 5 | (RX->raw[2] >> 3);
	RX->ch[2] = (int16_t)(RX->raw[5] & 0x01) << 10 | ((int16_t)RX->raw[4] << 2) | (RX->raw[3] >> 6);
	RX->ch[3] = (int16_t)(RX->raw[6] & 0x0F) << 7 | (RX->raw[5] >> 1);
	RX->ch[4] = (int16_t)(RX->raw[7] & 0x7F) << 4 | (RX->raw[6] >> 4);
	RX->ch[5] = (int16_t)(RX->raw[9] & 0x03) << 9 | ((int16_t)RX->raw[8] << 1) | (RX->raw[7] >> 7);
	RX->ch[6] = (int16_t)(RX->raw[10] & 0x1F) << 6 | (RX->raw[9] >> 2);
	RX->ch[7] = (int16_t)(RX->raw[11] << 3) | (RX->raw[10] >> 5);
	RX->ch[8] = (int16_t)(RX->raw[13] & 0x07) << 8 | RX->raw[12];
	RX->ch[9] = (int16_t)(RX->raw[14] & 0x3f) << 5 | (RX->raw[13] >> 3);
	RX->ch[10] = (int16_t)(RX->raw[16] & 0x01) << 10 | ((int16_t)RX->raw[15] << 2) | (RX->raw[14] >> 6);
	RX->ch[11] = (int16_t)(RX->raw[17] & 0x0F) << 7 | (RX->raw[16] >> 1);
	RX->ch[12] = (int16_t)(RX->raw[18] & 0x7F) << 4 | (RX->raw[17] >> 4);
	RX->ch[13] = (int16_t)(RX->raw[20] & 0x03) << 9 | ((int16_t)RX->raw[19] << 1) | (RX->raw[18] >> 7);
	RX->ch[14] = (int16_t)(RX->raw[21] & 0x1F) << 6 | (RX->raw[20] >> 2);
	RX->ch[15] = (int16_t)RX->raw[22] << 3 | (RX->raw[21] >> 5);

	return NO_ERR;
}

sbusError SBusEncode(uint16_t *raw) // 发射机部分代码仅保留接口，尚未实现
{
	/*
	// init SBUS SERIAL and config file
	static int SBUS_RDY;
	if (!SBUS_RDY)
	{
		// Serial 100K@8E2
		// UART_SBUS->BRR = 0x0168;//Baudrate = 100K
		// UART_SBUS->CR1 = 0x351c;//Even
		// UART_SBUS->CR2 = 0x2000;//Stop 2 bits
		for (int cnt = 0; cnt < 16; cnt++)
		{
			SBUS_cfg_file.bias[cnt] = 1024;
			SBUS_cfg_file.ifInvert[cnt] = 0;
			SBUS_cfg_file.ch_match[cnt] = cnt;
		}

		SBUS_RDY = 1;
	}

	// channel distribution
	uint16_t ch_data[16];
	for (int cnt = 0; cnt < 16; cnt++)
	{
		ch_data[cnt] = sbus_str[SBUS_cfg_file.ch_match[cnt]];
	}

	// header
	RX->raw[0] = 0x0F;

	// 16 ch_data of 11 bit data
	RX->raw[1] = (unsigned char)((ch_data[0] & 0x07FF));
	RX->raw[2] = (unsigned char)((ch_data[0] & 0x07FF) >> 8 | (ch_data[1] & 0x07FF) << 3);
	RX->raw[3] = (unsigned char)((ch_data[1] & 0x07FF) >> 5 | (ch_data[2] & 0x07FF) << 6);
	RX->raw[4] = (unsigned char)((ch_data[2] & 0x07FF) >> 2);
	RX->raw[5] = (unsigned char)((ch_data[2] & 0x07FF) >> 10 | (ch_data[3] & 0x07FF) << 1);
	RX->raw[6] = (unsigned char)((ch_data[3] & 0x07FF) >> 7 | (ch_data[4] & 0x07FF) << 4);
	RX->raw[7] = (unsigned char)((ch_data[4] & 0x07FF) >> 4 | (ch_data[5] & 0x07FF) << 7);
	RX->raw[8] = (unsigned char)((ch_data[5] & 0x07FF) >> 1);
	RX->raw[9] = (unsigned char)((ch_data[5] & 0x07FF) >> 9 | (ch_data[6] & 0x07FF) << 2);
	RX->raw[10] = (unsigned char)((ch_data[6] & 0x07FF) >> 6 | (ch_data[7] & 0x07FF) << 5);
	RX->raw[11] = (unsigned char)((ch_data[7] & 0x07FF) >> 3);
	RX->raw[12] = (unsigned char)((ch_data[8] & 0x07FF));
	RX->raw[13] = (unsigned char)((ch_data[8] & 0x07FF) >> 8 | (ch_data[9] & 0x07FF) << 3);
	RX->raw[14] = (unsigned char)((ch_data[9] & 0x07FF) >> 5 | (ch_data[10] & 0x07FF) << 6);
	RX->raw[15] = (unsigned char)((ch_data[10] & 0x07FF) >> 2);
	RX->raw[16] = (unsigned char)((ch_data[10] & 0x07FF) >> 10 | (ch_data[11] & 0x07FF) << 1);
	RX->raw[17] = (unsigned char)((ch_data[11] & 0x07FF) >> 7 | (ch_data[12] & 0x07FF) << 4);
	RX->raw[18] = (unsigned char)((ch_data[12] & 0x07FF) >> 4 | (ch_data[13] & 0x07FF) << 7);
	RX->raw[19] = (unsigned char)((ch_data[13] & 0x07FF) >> 1);
	RX->raw[20] = (unsigned char)((ch_data[13] & 0x07FF) >> 9 | (ch_data[14] & 0x07FF) << 2);
	RX->raw[21] = (unsigned char)((ch_data[14] & 0x07FF) >> 6 | (ch_data[15] & 0x07FF) << 5);
	RX->raw[22] = (unsigned char)((ch_data[15] & 0x07FF) >> 3);

	// flags
	RX->raw[23] = 0x00;
	// footer
	RX->raw[24] = 0x00;

	HAL_UART_Transmit_DMA(&UART_SBUS_HANDLE, RX->raw, 25);
	*/

	return FRM_ERROR;
}

uint16_t GetSbus(uint8_t queryChannel)
{
	if (queryChannel >= SBUS_CH_MAX)
	{
		return SBUS_CH_VAL_DEFAULT;
	}
	if (queryChannel < 1)
	{
		return SBUS_CH_VAL_DEFAULT;
	}
	return RX->ch[queryChannel - 1];
}

sbusComFailSta SBusIsComFail(void)
{
	return RX->comFailSta;
}
